package Roboter;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Tools;

public class Bewegung {
	private Rotationsachse rotationsachse;
	private Linearachse linearachse;
	private Hubservo hubservo;
	private RotationsPoti rotationsPoti;
	private LinearPoti linearPoti;
	private HubPoti hubPoti;
	private Elektromagnet elektromagnet;
	private boolean weiter;
	
	private static final int anschlagHubOben = 205;
	private static final int anschlagHubUnten = 125;
	private static final int bremsAbstandLinear = 40;
	private static final int bremsAbstandRotation = 40;
	private static final int bremsAbstandHub = 20;
	private static final int zeitVerzoegerung = 150;

	public Bewegung(Linearachse linearachse, LinearPoti linearPoti, Rotationsachse rotationsachse, RotationsPoti rotationsPoti, Hubservo hubservo, HubPoti hubPoti, Elektromagnet elektromagnet) {
		this.linearachse = linearachse;
		this.linearPoti = linearPoti;
		this.rotationsachse = rotationsachse;
		this.rotationsPoti = rotationsPoti;
		this.hubservo = hubservo;
		this.elektromagnet = elektromagnet;
		this.hubPoti = hubPoti;
	}
	
	
	public void startPosition() {
		Position startPostion = new Position(100, 10, false);
		
		// Linearachse auf Startposition ausrichten
		Thread bewegung1 = new BewegungLinear(startPostion);
		bewegung1.start();
		// Rotationsachse auf Startposition ausrichten
		Thread bewegung2 = new BewegungRotation(startPostion);
		bewegung2.start();
		
		weiter = true;
		while(weiter) {
			if(bewegung1.isInterrupted() && bewegung2.isInterrupted())
				weiter = false;
		}
		
	}


	public boolean wuerfelVerschieben(Position ausgangsPosition, Position zielPosition) {
		if (ausgangsPosition.hatWuerfel()) {
			
			// Linearachse auf Ausgangsposition ausrichten
			Thread bewegung1 = new BewegungLinear(ausgangsPosition);
			bewegung1.start();
			// Rotationsachse auf Ausgangsposition ausrichten
			Thread bewegung2 = new BewegungRotation(ausgangsPosition);
			bewegung2.start();
			
			weiter = true;
			while(weiter) {
				if(bewegung1.isInterrupted() && bewegung2.isInterrupted())
					weiter = false;
			}
			
			// Wuerfel aufheben
			zylinderAktion();
			
			// Linearachse auf Zielposition ausrichten
			bewegung1 = new BewegungLinear(zielPosition);
			bewegung1.start();
			// Rotationsachse auf Zielposition ausrichten
			bewegung2 = new BewegungRotation(zielPosition);
			bewegung2.start();
				
			weiter = true;
			while(weiter) {
				if(bewegung1.isInterrupted() && bewegung2.isInterrupted())
					weiter = false;
			}
			
			// Wuerfel ablegen
			zylinderAktion();
			
			//Wuerfel Positionen anpassen
			ausgangsPosition.wechselnWuerfel();
			zielPosition.wechselnWuerfel();
			return true;
		} else {
			System.out.println("Kein Wuerfel auf der Ausgangsposition");
			return false;
		}
	}
	
	public void hubAuf() {
		weiter = true;
		while(weiter) {
			if(hubPoti.gibAktuellenWert() >= anschlagHubOben) {
				this.hubservo.stopp();
				weiter = false;
			}/*
			else if(hubPoti.gibAktuellenWert() < anschlagHubOben - bremsAbstandHub) {
				this.hubservo.langsamAuf();
			}*/
			else if(hubPoti.gibAktuellenWert() < anschlagHubOben) {
				this.hubservo.auf();
			}
		}
	}
	
	public void hubAb() {
		weiter = true;
		while(weiter) {
			if(hubPoti.gibAktuellenWert() <= anschlagHubUnten) {
				this.hubservo.stopp();
				weiter = false;
			}/*
			else if(hubPoti.gibAktuellenWert() > anschlagHubUnten + bremsAbstandHub) {
				this.hubservo.langsamAb();
			}*/
			else if(hubPoti.gibAktuellenWert() > anschlagHubUnten) {
				this.hubservo.ab();
			}
		}
	}
	
	private void zylinderAktion() {
		hubAb();
		try {
			Thread.sleep(zeitVerzoegerung);
		} catch (InterruptedException e) {
			
		}
		this.elektromagnet.schalten();
		try {
			Thread.sleep(zeitVerzoegerung);
		} catch (InterruptedException e) {
			
		}
		hubAuf();
	}


	// Linearachse auf Position ausrichten
	private class BewegungLinear extends Thread {
		private Position ausgangsPosition;
		public BewegungLinear(Position position) {
			ausgangsPosition = position;
		}
		
		public void run() {
			while(!isInterrupted()) {
				if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate()) {
					if (linearPoti.gibAktuellenWert() >= ausgangsPosition.gibAchsenKoordinate()) {
						linearachse.stopp();
						interrupt();
					}
					else if (linearPoti.gibAktuellenWert() > ausgangsPosition.gibAchsenKoordinate() - bremsAbstandLinear) {
						linearachse.langsamVor();
					}
					else if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate()) {
						linearachse.schnellVor();
					}
				} else {
					if (linearPoti.gibAktuellenWert() <= ausgangsPosition.gibAchsenKoordinate()) {
						linearachse.stopp();
						interrupt();
					}
					else if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate() + bremsAbstandLinear) {
						linearachse.langsamZurueck();
					}
					else if (linearPoti.gibAktuellenWert() > ausgangsPosition.gibAchsenKoordinate()) {
						linearachse.schnellZurueck();
					}
				}
			}
		}
	}
	
	// Rotationsachse auf Position ausrichten
	private class BewegungRotation extends Thread {
		private Position ausgangsPosition;
		public BewegungRotation(Position position) {
			ausgangsPosition = position;
		}
		
		public void run() {
			while(!isInterrupted()) {
				if(rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate()) {
					if (rotationsPoti.gibAktuellenWert() >= ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.stopp();
						interrupt();
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibErsteRotationsKoordinate() - bremsAbstandRotation) {
						rotationsachse.langsamVor();
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.schnellVor();
					}
				} else {
					if (rotationsPoti.gibAktuellenWert() <= ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.stopp();
						interrupt();
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate() + bremsAbstandRotation) {
						rotationsachse.langsamZurueck();
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.schnellZurueck();
					}
				}
			}
		}
	}
}
